// standard libs
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>


#include "ff_utils.h"
#include "main.h"
#include "../utils/util.h"
#include "../utils/button.h"
#include "../utils/LCD_driver.h"
#include "../utils/LCD_functions.h"


void ExitRoom(void)
{

	while (GetADC(FrontLineSensor) > 0x80) // Drive until line on the floor
	{	

	//Turn sharp if too close to wall
		if ((GetADC(LeftDistSensor) > 0x38))
		{	
			SetLeftMotorPWM(10);
			SetRightMotorPWM(-7);
		}
		else if ((GetADC(RightDistSensor) > 0x38))
		{
			SetLeftMotorPWM(-7);
			SetRightMotorPWM(10);
		}
	//Steer out of the room	
		else if (GetADC(LeftDistSensor) > (GetADC(RightDistSensor)))	
		{	
			SetLeftMotorPWM(10);
			SetRightMotorPWM(5);		
		}
		else 
		{					
			SetLeftMotorPWM(5);
			SetRightMotorPWM(10);
		}
	}
//Reached the threshold
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);	
}

/*********************************************************************
*	 goes to the center of room and checks corners
**********************************************************************/

void FlameDemo(void)
{
	while (!(getkey() == 1))
	{
	unsigned char	HotPixel,
					Temperature;
		GetHotPixel(&HotPixel,&Temperature);

		if(Temperature > 6) // If there is a flame, returns true
		{
			show2hex(HotPixel,Temperature);
			sendNumber(HotPixel);
			sendChar(',');
			sendNumber(Temperature);
			sendString("\r\n");
			if (HotPixel == 1)
			{
				//LCD_puts_f(PSTR("pixel 1"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(7);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 1");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}
			else if (HotPixel == 2 )
			{
				//LCD_puts_f(PSTR("pixel 2"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(8);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 2");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}
			else if (HotPixel == 3 )
			{
			//	LCD_puts_f(PSTR("pixel 3"),1);
			//	SetRightMotorPWM(0);
			//	SetLeftMotorPWM(0);		
			//	TimerWait(1000);
				SetRightMotorPWM(9);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 3");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");			
			}			
			else if (HotPixel == 4 )
			{
			//	LCD_puts_f(PSTR("pixel 4"),1);
				//SetRightMotorPWM(0);
			//	SetLeftMotorPWM(0);
			//	TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 4");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}						
			else if (HotPixel == 5 )
			{
			//	LCD_puts_f(PSTR("pixel 5"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 5");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}
			else if (HotPixel == 6)
			{
				//LCD_puts_f(PSTR("pixel 6"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(9);
				TimerWait(10);
				sendString("Pixel 6");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}						
			else if (HotPixel == 7)
			{
				//LCD_puts_f(PSTR("pixel 7"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(8);
				TimerWait(10);
				sendString("Pixel 7");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}		
			else if (HotPixel == 8)
			{
				//LCD_puts_f(PSTR("pixel 8"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(7);
				TimerWait(10);
				sendString("Pixel 8");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}

			/*if(PINB & 0x20)				// check for balloon pop
			{
				SetRightMotorPWM(0);
				SetLeftMotorPWM(0);
				TimerWait(1000);
				break;
			*/

			else
			{
				break;
			}
		}
		else
		{
			SetRightMotorPWM(7);
			SetLeftMotorPWM(-7);
			LCD_puts_f(PSTR("no"),1);
			sendString("No");		
			sendString("\r\n");
		}
	}
}


/*********************************************************************
*	 goes to the center of room and checks corners
**********************************************************************/

void CenterRoomFlame(void)
{

	while (!(getkey() == 1))
	{
		LCD_puts_f(PSTR("drive"),1);
		SetRightMotorPWM(10);
		SetLeftMotorPWM(9);
		TimerWait(1000);
		SetRightMotorPWM(9);
		SetLeftMotorPWM(10);
		TimerWait(1000);
		SetRightMotorPWM(10);
		SetLeftMotorPWM(10);
		TimerWait(250);
		SetRightMotorPWM(0);
		SetLeftMotorPWM(0);
		TimerWait(1000);
		
		if(CheckForFlame())
		{
		GoToFlame();
		}
		else
		{
		LCD_puts_f(PSTR("no flame found"),1);
		}
	}
}


int CheckTerminatingConditions(int TerminatingCondition,unsigned long TerminatingValue)
{
	unsigned char adcL, adcM, adcR;

	adcR = GetADC(RightDistSensor);
	adcM = GetADC(MiddleDistSensor);
	adcL = GetADC(LeftDistSensor);

	switch(TerminatingCondition)
	{
	case UNTIL_KEYPRESS:
		if(getkey() == TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_RIGHTSENSOR_GREATER:
		if(GetADC(RightDistSensor) > TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_MIDDLESENSOR_GREATER:
		if(GetADC(MiddleDistSensor) > TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_LEFTSENSOR_GREATER:
		if(GetADC(LeftDistSensor) > TerminatingValue)
			return(TRUE);
		
		break;
	
	case UNTIL_LINESENSOR_GREATER:
		if(GetADC(FrontLineSensor) > TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_RIGHTSENSOR_LOWER:
		if(GetADC(RightDistSensor) < TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_MIDDLESENSOR_LOWER:
		if(GetADC(MiddleDistSensor) < TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_LEFTSENSOR_LOWER:
		if(GetADC(LeftDistSensor) < TerminatingValue)
			return(TRUE);
		
		break;
	
	case UNTIL_LINESENSOR_LOWER:
		if(GetADC(FrontLineSensor) < TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_TIME:
		if(TimerMsCur() > TerminatingValue)
			return(TRUE);
		
		break;
	}

	return(FALSE);

}


/*********************************************************************
*	 Test sequence for the PWM code Assumes IXDN Chip
**********************************************************************/
void PWM_Test(void)
{
    while (!(getkey() == 1))
    { 	 
	    LCD_puts_f(PSTR("Left"),1);
	    SetLeftMotorPWM(-10);
        SetRightMotorPWM(10);
        TimerWait(3000);
		
		LCD_puts_f(PSTR("Right"),1);	
	    SetLeftMotorPWM(10);
        SetRightMotorPWM(-10);
        TimerWait(3000);
		
        LCD_puts_f(PSTR("2"),1);
	    SetLeftMotorPWM(2);
        SetRightMotorPWM(2);
        TimerWait(3000);
		
		LCD_puts_f(PSTR("4"),1);	
	    SetLeftMotorPWM(4);
        SetRightMotorPWM(4);
        TimerWait(3000);
	 
        LCD_puts_f(PSTR("6"),1);
	    SetLeftMotorPWM(6);
        SetRightMotorPWM(6);
        TimerWait(3000);
		
		LCD_puts_f(PSTR("8"),1);	
	    SetLeftMotorPWM(8);
        SetRightMotorPWM(8);
        TimerWait(3000);
	 
        LCD_puts_f(PSTR("10"),1);
	    SetLeftMotorPWM(10);
        SetRightMotorPWM(10);
        TimerWait(3000);
	
		LCD_puts_f(PSTR("-2"),1);	
	    SetLeftMotorPWM(-2);
        SetRightMotorPWM(-2);
        TimerWait(3000);
	 
        LCD_puts_f(PSTR("-4"),1);
	    SetLeftMotorPWM(-4);
        SetRightMotorPWM(-4);
        TimerWait(3000);

		LCD_puts_f(PSTR("-6"),1);	
	    SetLeftMotorPWM(-6);
        SetRightMotorPWM(-6);
        TimerWait(3000);
	 
        LCD_puts_f(PSTR("-8"),1);
	    SetLeftMotorPWM(-8);
        SetRightMotorPWM(-8);
        TimerWait(3000);
		
		LCD_puts_f(PSTR("-10"),1);	
	    SetLeftMotorPWM(-10);
        SetRightMotorPWM(-10);
        TimerWait(3000);
	}
    // Done
		SetLeftMotorPWM(0);
        SetRightMotorPWM(0);
}

/*************************************************************************
*
* 	Threshold Test
*
***************************************************************************/

void Thresholdtest(void)
{
	
	unsigned char adcLine;
	
    while (!(getkey() == 1))
    { 
//	SetLeftMotorPWM(10);
//    SetRightMotorPWM(10);
	//adcLine = GetADC(FrontLineSensor);
	//show1hex(adcLine);
	//sendNumber(adcLine);
	sendChar('X');
	//sendString("\r");
	//sendNumber(adcLine);
	//sendString("\r\n");
	//TimerWait(100);
	}
SetLeftMotorPWM(0);
SetRightMotorPWM(0);
}


